#ifndef QUATERNION_H_
#define QUATERNION_H_

#include "Fixed.h"
#include "Matrix.h"
#include "Vector3.h"

namespace uav
{
namespace math
{

/**
 * @class Quaternion
 *
 * @brief Fixed-point quaternion class.
 *
 * This class represents quaternions of the form @f$ w + x i + y j + z k@f$.
 *
 * @author Travis Portz
 */
class Quaternion
{
public:
	Fixed w; ///< The scalar component of the quaternion.
	Fixed x; ///< The i-component of the quaternion.
	Fixed y; ///< The j-component of the quaternion.
	Fixed z; ///< The k-component of the quaternion.

	/**
	 * Constructs a zero quaternion.
	 */
	Quaternion() {}

	/**
	 * Constructs a quaternion with the given components.
	 */
	Quaternion(const Fixed &w, const Fixed &x, const Fixed &y, const Fixed &z) : w(w), x(x), y(y), z(z) {}

	/**
	 * Constructs a quaternion with the given scalar and vector components.
	 */
	Quaternion(const Fixed &w, const Vector3 &v) : w(w), x(v.x), y(v.y), z(v.z) {}

	/**
	 * Obtains a unit quaternion from an axis-angle representation of a rotation.
	 */
	static Quaternion fromAxisAngle(const Vector3 &axis, const Fixed &angle);

	/**
	 * Converts the quaternion (assumed to be of unit length) to an axis-angle representation of a rotation.
	 *
	 * @param[out] axis  The quaternion's axis of rotation.
	 * @param[out] angle The quaternion's angle of rotation (in radians).
	 */
	void toAxisAngle(Vector3 &axis, Fixed &angle) const;

	/**
	 * Converts the quaternion (assumed to be of unit length) to the DCM matrix representation of the rotation.
	 */
	Matrix toMatrix() const;

	/**
	 * Converts the quaternion (assumed to be of unit length) to the transpose of the DCM matrix representation of the rotation.
	 *
	 * This matrix can be used to rotate by the inverse of the quaternion and its DCM matrix representation without
	 * needing to take the inverse or transpose directly.
	 */
	Matrix toMatrixTranspose() const;

	/**
	 * Computes the length of the quaternion squared.
	 *
	 * This method is faster than computing the length and then squaring it.
	 * No square root operation is used.
	 */
	Fixed lengthSquared() const { return w * w + x * x + y * y + z * z; }

	/**
	 * Computes the length of the quaternion.
	 */
	Fixed length() const { return sqrt(lengthSquared()); }

	/**
	 * Normalizes the length of this quaternion to one.
	 */
	void normalize()
	{
		Fixed s = length();
		if (s == Fixed::Zero) return;
		s = Fixed::One / s;
		w *= s; x *= s; y *= s; z *= s;
	}

	/**
	 * Returns a normalized version of the quaternion.
	 *
	 * This method does not modify the original quaternion.
	 *
	 * @see normalize()
	 */
	Quaternion normalized() const
	{
		Quaternion temp = *this;
		temp.normalize();
		return temp;
	}

	/**
	 * Returns the conjugate of the quaternion.
	 */
	Quaternion conjugate() const
	{
		return Quaternion(w, -x, -y, -z);
	}

	/**
	 * Returns the quaternion squared.
	 */
	Quaternion square() const;

	/**
	 * Returns the exponential of the quaternion.
	 *
	 * For @f$ q = (w, x, y, z) = (w, \mathbf{v}) @f$, we have:
	 * @f[ e^q = e^w (\cos \lVert \mathbf{v} \rVert, \frac{\mathbf{v}}{\lVert \mathbf{v} \rVert} \sin \lVert \mathbf{v} \rVert) @f]
	 */
	Quaternion exp() const;

	/**
	 * Returns the logarithm of the quaternion.
	 *
	 * Let @f$ q = r (\cos \theta, \mathbf{u} \sin \theta) = r e^{(0, \theta \mathbf{u})} @f$
	 * where @f$ r = \lVert q \rVert @f$ and @f$ \lVert \mathbf{u} \rVert = 1 @f$.
	 * Then @f$ \log q = (\log r, \theta \mathbf{u}) @f$.
	 */
	Quaternion log() const;

	/**
	 * Returns the quaternion to the power @a k.
	 *
	 * For quaternion @f$ q @f$, we have @f$ q^k = e^{k \log q} @f$.
	 */
	Quaternion pow(const Fixed &k) const;

	/**
	 * Rotates a vector by the quaternion (assumed to be a unit quaternion).
	 *
	 * @note If you are rotating multiple vectors, it is more efficient to convert
	 * the quaternion to a matrix and transform the vectors by the matrix.
	 */
	Vector3 rotate(const Vector3 &v) const;

	/**
	 * Rotates a vector by the inverse of this quaternion (assumed to be a unit quaternion).
	 *
	 * This operation is done without actually computing the inverse of the quaternion.
	 *
	 * @note If you are rotating multiple vectors, it is more efficient to convert
	 * the quaternion to a matrix and transform the vectors by the matrix.
	 */
	Vector3 rotateInverse(const Vector3 &v) const;

	Quaternion operator*(const Quaternion &q) const;
	Quaternion& operator*=(const Quaternion &q)
	{
		*this = (*this) * q; // the multiplication can't be done in place
		return *this;
	}

	Quaternion& operator+=(const Quaternion &q)
	{
		w += q.w; x += q.x; y += q.y; z += q.z;
		return *this;
	}
	Quaternion& operator-=(const Quaternion &q)
	{
		w -= q.w; x -= q.x; y -= q.y; z -= q.z;
		return *this;
	}

	Quaternion operator+(const Quaternion &q) const
	{
		Quaternion temp = *this;
		temp += q;
		return temp;
	}
	Quaternion operator-(const Quaternion &q) const
	{
		Quaternion temp = *this;
		temp -= q;
		return temp;
	}

	Quaternion& operator*=(const Fixed &s)
	{
		w *= s; x *= s; y *= s; z *= s;
		return *this;
	}
	Quaternion& operator/=(const Fixed &s)
	{
		w /= s; x /= s; y /= s; z /= s;
		return *this;
	}

	Quaternion operator*(const Fixed &s) const
	{
		Quaternion temp = *this;
		temp *= s;
		return temp;
	}
	Quaternion operator/(const Fixed &s) const
	{
		Quaternion temp = *this;
		temp /= s;
		return temp;
	}

	Quaternion operator-() const
	{
		return Quaternion(-w, -x, -y, -z);
	}

	friend Quaternion operator*(const Fixed &s, const Quaternion &q)
	{
		Quaternion temp = q;
		temp *= s;
		return temp;
	}
};

}}

#endif
